#!env python
# -*- coding: utf-8 -*-
#	 2010-07-16
#
#	安全制动模型的相关计算
# 
# Logs:
#	v0.1 
#
import sys
import math

def tocms(v):
	return v/0.036

def tokmh(v):
	return v*0.036

MAX_SPEED=88/0.036
MAX_TARGET_S=35000
ENABLE_DEBUG=0
LINE_SPEED=80

class core_module():
	def __init__(self, ref_acc, real_acc, v_safe, v0):
		# 牵引加速度曲线
		self.tract_a = [89,89,89,89,89,89,89,89,89,89,76,76,57,57,39,39,29,29,29,29,29]
		self.ref_acc = ref_acc*100
		self.real_acc = real_acc*100
		self.v_safe = v_safe/0.036
		self.v0 = v0/0.036
		self.step = 0.1	# 0.1s
		self.a3 = self.ref_acc/2
		self.ato_acc = 0
		self.counter = 0
		self.s_cut = 0
		self.s_safe = 0
		self.s_eb = 0
		self.s_bad = 0
		self.a1 = 0
		self.critical_v = 0
		self.ceiling_v = 0
		self.target_limit_v = 0
		self.max_ramp_a = 30
		self.trigger_time_thr = 2
		self.s0 = 0

    def load_xml(self, filename):
        pass

    def save_xml(self, filename):
        pass

	def get_tract_acc(self, v):
		tempi = int(v*0.036/5)
		if (tempi < len(self.tract_a)):		
			if (tempi < 0):
				tempi = 0
			return self.tract_a[tempi]
		else:
			return self.tract_a[-1]

	def set_param(self, ramp_a, t1, t2, t3, a1, a2):
		self.ramp_a = ramp_a
		self.t1 = t1
		self.t2 = t2
		self.t3 = t3
		self.a1 = a1*100
		self.a2 = a2*100

	def set_param2(self, ramp_a, t1, t2, t3, a1, a2, ato_ref):
		self.ramp_a = ramp_a
		self.t1 = t1
		self.t2 = t2
		self.t3 = t3
		self.a1 = a1*100
		self.a2 = a2*100
		self.ato_acc = ato_ref*100

	def get_overlap(self, cur_v):
		# 计算保护区段, 根据当前速度来计算保护区段
		# 其他方向
		#s = //self.
		return 0

	def get_decelerate_speed(self, target_s, target_v):
		# 计算匀减速的触发速度
		return math.sqrt(target_v*target_v-2*(self.ref_acc+self.ramp_a)*target_s)

	def get_ceiling_speed_cms(self, limit_speed):
		# 计算顶棚条件下的速度触发数值
		self.ceiling_v = limit_speed - (self.get_tract_acc(limit_speed)+self.ramp_a)*self.t1 - (self.a2+self.ramp_a)*self.t2
		return self.ceiling_v

	def get_ceiling_speed(self, limit_speed):
		return tokmh(self.get_ceiling_speed_cms(tocms(limit_speed)))

	# 计算IEEE1474触发速度
	def get_critical_speed_cms(self, target_s, target_v):
		# 计算核心的触发速度
		v1=self.get_decelerate_speed(target_s, target_v)
		self.a1 = self.get_tract_acc(v1)
		self.critical_v = v1 + (self.ref_acc)*(self.t1 + self.t2 + self.t3) \
				- self.a1*self.t1 \
				- self.a2 * self.t2  \
				- self.a3*self.t3
		if self.critical_v < 0:
			self.critical_v = 0
		if ENABLE_DEBUG==1:
			print "v1:",v1,tokmh(v1),"tv:",self.critical_v,tokmh(self.critical_v)
		return self.critical_v

	def get_critical_speed(self, target_s, target_v):
		# 计算核心的触发速度
		target_s = target_s
		target_v = tocms(target_v)
		if ENABLE_DEBUG==1:
			print "tgt s:",target_s,"tgt v:",target_v
		return tokmh(self.get_critical_speed_cms(target_s, target_v))

	# 获取前方存在目标点时的触发速度
	def get_target_limit_speed_cms(self, target_s, target_v, limit_v):
		if target_s>MAX_TARGET_S:
			return self.get_ceiling_speed_cms(limit_v)
		else:
			tempv = self.get_critical_speed_cms(target_s, target_v)
			ceiling_v = self.get_ceiling_speed_cms(limit_v)
			if ENABLE_DEBUG==1:
				print "tgt limit", tempv, ceiling_v
			if (tempv > ceiling_v):
				tempv = ceiling_v
			if (self.s0 == 0 and tempv < ceiling_v):
				self.s0 = target_s
				self.v0 = self.get_op_speed(self.get_safe_speed(tempv))
			self.target_limit_v = tempv
			return self.target_limit_v

	def get_target_limit_speed(self, target_s, target_v, limit_v):
		v = self.get_target_limit_speed_cms(target_s, tocms(target_v), tocms(limit_v))
		return tokmh(v)

	def get_safe_speed(self, v):
		# 考虑平坡加速
		tempv = v - self.max_ramp_a*self.trigger_time_thr
		return tempv

	def get_op_speed(self, v):
		tempv = v - tocms(2.5)
		return tempv

	def get_all_speed(self, target_s, target_v):
		target_v = tocms(target_v)
		v1 = self.get_decelerate_speed(target_s, target_v)
		if (v1>MAX_SPEED):
			v1 = MAX_SPEED
		v2 = self.get_target_limit_speed_cms(target_s, target_v, tocms(LINE_SPEED))
		v3 = self.get_safe_speed(v2)
		if (self.s0 == 0):
			v4 = self.get_op_speed(v3)
		else:
			temp = self.v0*self.v0 + 2.0*self.ato_acc*(self.s0 - target_s)
			if temp>0:
				v4 = math.sqrt(temp)
			else:
				v4 = 0
		print '%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f	%3.1f'%\
				(target_s/100.0, tokmh(v1), tokmh(v2), tokmh(v3), tokmh(v4), \
				v1, v2, v3)

	def get_critical_s(self):
		# 获取ieee1474触发曲线和终点的距离
		a1 = self.get_tract_acc(0)
		self.a1 = a1
		v2 = (self.ramp_a+a1)*self.t1
		v3 = v2 + (self.a2 + self.ramp_a)*self.t2
		v4 = v3 + (self.a3 + self.ramp_a)*self.t3
		#print v2, v3, v4
		s = (a1+self.ramp_a)*self.t1*self.t1/2.0 \
				+ v2*self.t2 + (self.a2 + self.ramp_a)*self.t2*self.t2/2.0 \
				+ v3*self.t3 + (self.a3 + self.ramp_a)*self.t3*self.t3/2.0 \
				- v4*v4/(self.ref_acc+self.ramp_a)/2
		return s

	def get_loop_all_speed(self, s1, target_v):
		if (s1>100):
			self.report()
			self.get_all_speed(s1, target_v)
			#print '距离	匀减速触发	1474EB触发	全常用制动	运营速度'
			#for i in range(int(s1), -1, -100):
			#	self.get_all_speed(i, target_v)

	def get_trigger_speed(self, s, hasoverlap):
		# 获取安全相关速度
		return 0

	def report(self):
		print '# t1:', self.t1, ' t2:', self.t2, ' t3:', self.t3, 'a2:', self.a1, self.a2, 'a3:', self.ref_acc/2, 'ramp_a:', self.ramp_a
		print '# REF_A:',self.ref_acc, ' REAL_A:', self.real_acc, 'TRIGGER_V:', self.v_safe*0.036, 'delta_s', self.get_critical_s()

if __name__ == "__main__":
	# 输入
	ref_a = -0.8
	ato_acc = -0.8
	# 紧急制动实际减速度
	real_a = -0.96
	v0 = 90
	v = 75.0
	verr = 2.5
	t1 = 1.5	# 切除牵引时间
	t2 = 0	# 惰行时间
	t3 = 0.6	# eb建立时间
	a1 = 0.89
	a2 = 0
	ramp_a =0
	s = 40000

	# 支持多个参数
	i=1
	if (len(sys.argv) > i):
		s = float(sys.argv[i])*100
		i=i+1
	if (len(sys.argv) > i):
		v0 = float(sys.argv[i])
		i=i+1
	if (len(sys.argv) > i):
		v = float(sys.argv[i])
		i=i+1
	if (len(sys.argv) > i):
		ramp_a = int(sys.argv[i])
		i=i+1
	if (len(sys.argv) > i):
		ref_a = float(sys.argv[i])
		if (ref_a > 0):
			ref_a = -0.8
		i=i+1
	if (len(sys.argv) > i):
		real_a = float(sys.argv[i])
		if (real_a > 0):
			real_a = -1.0
		i=i+1
	if (len(sys.argv) > i):
		t1 = float(sys.argv[i])
		i=i+1
	if (len(sys.argv) > i):
		t2 = float(sys.argv[i])
		i=i+1
	if (len(sys.argv) > i):
		t3 = float(sys.argv[i])
		i=i+1

	module = core_module(ref_a, real_a, v0, v)
	module.set_param2(ramp_a, t1, t2, t3, a1, a2, ato_acc)
	#module.get_loop_all_speed(s, v0);
	print "%1.1f"%(module.get_ceiling_speed(v0)-5.5)

